#include "volAND.h"
#include "projector.h"
#include "visualizer.h"

#include <mmintrin.h>
#include <xmmintrin.h>

#include <memory.h>
// for a given Projection P, call test to all of it's children node, and combine the score

void volAND::test(float Pmat[][4], float cam_dir[3], float &total_score, std::deque<int> &deformations){



	int n_child = this->children.size();
	deformations.clear();
	total_score = 0; 
	for( int i_chd = 0 ; i_chd<n_child;++i_chd){
		float score; int arg_max;
		score = 0; arg_max = -1;
		this->children[i_chd]->test(Pmat,cam_dir,score,arg_max);	
		total_score+=score;
		deformations.push_back(arg_max);
	}
	assert(n_child==deformations.size());
}


/*create particle list, from particles of child node */
void volAND::inference(){
	this->vol_particle_list.clear();
	tanOR *p_tan; 
	int n_tan = this->children.size();

	for(int i_tan = 0; i_tan<n_tan; ++i_tan){
		p_tan = children[i_tan];
		int n_ptc=p_tan->tan_particle_list.size();
		for( int i_ptc=0; i_ptc<n_ptc; ++i_ptc){
			//tanParticle_s &tan_ptc = p_tan->tan_particle_list[i_ptc];			
			tanParticle_s tan = p_tan->tan_particle_list.front();
			volParticle_s vol_ptc;
			this->test(tan.P,tan.cam_dir,vol_ptc.score,vol_ptc.deform_list);
			for( int row = 0; row<3; ++row){
				memcpy(vol_ptc.P[row],const_cast<const float*>(tan.P[row]),sizeof(float)*4);
			}
			memcpy(vol_ptc.cam_dir,const_cast<const float*>(tan.cam_dir),sizeof(float)*3);
			vol_ptc.id = this->id;
			vol_ptc.fit_error = -1;
			/*check and update the vol_and_particle_list*/
			this->updateParticleList(vol_ptc);

			p_tan->tan_particle_list.pop_front();
		}
	}
}

/*
* Directly evaluate every possible proposal of child nodes, 
* by calling their propose function, and evaluate the proposal 
* by calling myown test function, then upate my own particle list
*/

bool volAND::inferenceV2(const line &l1,const line &l2){

	lineVertex line1=p_img_data->matlabLine2lineVertex(l1);
	lineVertex line2=p_img_data->matlabLine2lineVertex(l2);
	return this->inferenceV2(line1,line2);
}


bool volAND::inferenceV2(const lineVertex &line1, const lineVertex &line2){

	//char window_name[100];
	//sprintf(&window_name[0],"volume: %d",this->id);
	//visualizer window(const_cast<const char*>(&window_name[0]));
	//window.setBackground(p_img_data->sy,p_img_data->sx,p_img_data->img_r,p_img_data->img_g,p_img_data->img_b);

	int n_child = this->children.size();
	tanParticle_s ptc1,ptc2;
	volParticle_s vol_ptc;
	for(int i_child=0; i_child<n_child; ++i_child){
		ptc1.pt_pairs.clear();
		ptc1.pt_pairs.clear();
		children[i_child]->proposeParticle(line1,line2,ptc1,ptc2);

		/*elevate from tan particle to volume particle, particle 1*/
		for( int row = 0; row<3; ++row){
			memcpy(vol_ptc.P[row],const_cast<const float*>(ptc1.P[row]),sizeof(float)*4);
		}
		memcpy(vol_ptc.cam_dir,ptc1.cam_dir,sizeof(ptc1.cam_dir[0])*3);
		vol_ptc.deform_list.clear();
		this->test(ptc1.P,ptc1.cam_dir,vol_ptc.score,vol_ptc.deform_list);
		vol_ptc.id = this->id;
		vol_ptc.fit_error = -1;

		//window.cleanImage();
		//window.drawLinePairs(ptc1.pt_pairs,15,2);
		//window.drawP(ptc1.P);
	

		this->updateParticleList(vol_ptc);


		/*elevate from tan particle to volume particle, particle 2*/
		for( int row = 0; row<3; ++row){
			memcpy(vol_ptc.P[row],const_cast<const float*>(ptc2.P[row]),sizeof(float)*4);
		}
		memcpy(vol_ptc.cam_dir,ptc2.cam_dir,sizeof(float)*3);
		vol_ptc.deform_list.clear();
		this->test(ptc1.P,ptc1.cam_dir,vol_ptc.score,vol_ptc.deform_list);
		vol_ptc.id = this->id;
		vol_ptc.fit_error = -1;


		//window.cleanImage();
		//window.drawLinePairs(ptc2.pt_pairs,15,2);
		//window.drawP(ptc2.P);

		this->updateParticleList(vol_ptc);

	
	}
	
	//window.release();
	return true;
}




bool volAND::updateParticleList(volParticle_s & particle){
	if( particle.score<-1e6) return false;
	if( this->vol_particle_list.empty()){
		vol_particle_list.push_back(particle);
		return true;
	}
	std::list<volParticle_s>::iterator itr = vol_particle_list.begin();
	for(;itr!=vol_particle_list.end(); ++itr){
		if(particle.score>=itr->score){
			vol_particle_list.insert(itr,particle);
			if(vol_particle_list.size()>PR_QUEUE_SIZE){
				vol_particle_list.pop_back();
			}
			return true;
		}
	}
	if(vol_particle_list.size()<PR_QUEUE_SIZE){
		vol_particle_list.push_back(particle);
		return true;
	}
	return false;

}

/*
* For high score particles, re-find the 3D and 2D point correspondense,
* and re-estimate the P;
*/
void volAND::argMax(volParticle_s &particle, float *p_template ){

	particle.tan_particle_list.clear();
	int n_chd = this->children.size();
	assert(n_chd == particle.deform_list.size());
	float score =0;
	for(int i_chd =0; i_chd<n_chd;++i_chd){
		tanParticle_s tan_ptc;
	//	printf("vol_id %d,  i_chd %d, child_id %d\n",this->id,i_chd,children[i_chd]->getID());
		this->children[i_chd]->argMax2(particle.P,particle.cam_dir,particle.deform_list[i_chd],tan_ptc,p_template);
		particle.tan_particle_list.push_back(tan_ptc);
		particle.deform_list[i_chd]=tan_ptc.arg_max_deformation;
		assert(tan_ptc.arg_max_deformation>-2);
		score +=tan_ptc.score;
	}

	/*for current particle, re-estimate Projection matrix*/
/*
	std::deque<pt_pair_s> pts; 
	float Pmat[3][4];
	float cam_dir[3];
	float error;
	if(n_chd>1){
		for(int i_chd =0; i_chd<n_chd;++i_chd){
			int n_pair=particle.tan_particle_list[i_chd].pt_pairs.size();
			for(int i_pair = 0; i_pair<n_pair; ++i_pair){
				pts.push_back(particle.tan_particle_list[i_chd].pt_pairs[i_pair]);
			}
		}

		projector::estimatePandDir(pts,Pmat,cam_dir,error);
		for(int row = 0; row<3; ++row) {
			memcpy(particle.P[row],Pmat[row],sizeof(float)*4);
		}
		memcpy(particle.cam_dir,cam_dir,sizeof(float)*3);
		particle.fit_error = error;
	}
*/
	particle.id = this->id;
	particle.score=score;
}

void volAND::updateImageDataRef(imgData *p){
	this->p_img_data=p;
}

float *volAND::SWTest(view_s &view, bool use_color){
	int sx = this->p_img_data->sx;
	int sy = p_img_data->sy;

	int n_child = children.size();


	float *total_score_map = new float[sx*sy];
	/*could be optimized*/
	for(int i_pix = 0; i_pix<sx*sy; ++i_pix) 
		total_score_map[i_pix] = 0.0f;
	
	for( int i_node = 0; i_node<this->children.size(); ++ i_node){
		/*call child tan_OR nodes' SWTestFunction*/
		float *tmp_score_map = this->children[i_node]->SWTest(view,use_color);
		/*Add returned score map together*/
#ifndef USE_SIMD
		for(int i_pix = 0; i_pix<sx*sy; ++i_pix)
			total_score_map[i_pix]+=tmp_score_map[i_pix];
#else
		__m128 sumDest,sumSource;
		int i_stop = sx*sy-3;
		int i_pix = 0;
		for(; i_pix<i_stop; i_pix+=4){
				sumDest = _mm_loadu_ps(&(total_score_map[i_pix]));
				sumSource=_mm_loadu_ps(&(tmp_score_map[i_pix]));
				sumDest = _mm_add_ps(sumDest,sumSource);
				_mm_storeu_ps(&(total_score_map[i_pix]),sumDest);
		}
		i_pix-=4;
		for(;i_pix<sx*sy; ++i_pix){
			total_score_map[i_pix]+=tmp_score_map[i_pix];
		}
#endif
		/*release memory generated from child nodes*/
		delete[] tmp_score_map;
	}
		for( int i_pix = 0; i_pix<sx*sy; ++i_pix){
               // if(fabs(total_score_map[i_pix])>1e6)
               //         total_score_map[i_pix]=0.0f;
        }
	return total_score_map;
}
